#include "pt_config.h"
#include "pt_mem.h"
#include "ptdef.h"
#include "ptdebug.h"
#include "pt_ipc.h"

#include "my_utils.h"

#ifdef PT_USING_MEM_HEAP

#define HEAP_MAGIC 0x1ea0
struct heap_mem
{
    /* magic and used flag */
    uint16_t magic;
    uint16_t used;

    uint32_t next, prev;

};

/** pointer to the heap: for alignment, heap_ptr is now a pointer instead of an array */
static uint8_t *heap_ptr;

/** the last entry, always unused! */
static struct heap_mem *heap_end;

#define MIN_SIZE 12
#define MIN_SIZE_ALIGNED     PT_ALIGN(MIN_SIZE, PT_ALIGN_SIZE)
#define SIZEOF_STRUCT_MEM    PT_ALIGN(sizeof(struct heap_mem), PT_ALIGN_SIZE)

static struct heap_mem *lfree;   /* pointer to the lowest free block */

static struct pt_sem heap_sem = {   //内存堆信号量, 初始化为1, 即未被占用
    .suspend_thread.next = &heap_sem.suspend_thread,
    .suspend_thread.prev = &heap_sem.suspend_thread,
    .count = 1,
};
static uint32_t mem_size_aligned;

/**
 * @ingroup SystemInit
 *
 * This function will initialize system heap memory.
 *
 * @param begin_addr the beginning address of system heap memory.
 * @param end_addr the end address of system heap memory.
 */
void pt_system_heap_init(void *begin_addr, void *end_addr)
{
    struct heap_mem *mem;
    uint32_t begin_align = PT_ALIGN((uint32_t)begin_addr, PT_ALIGN_SIZE);
    uint32_t end_align = PT_ALIGN_DOWN((uint32_t)end_addr, PT_ALIGN_SIZE);

    /* alignment addr */
    if ((end_align > (2 * SIZEOF_STRUCT_MEM)) &&
        ((end_align - 2 * SIZEOF_STRUCT_MEM) >= begin_align)){
        /* calculate the aligned memory size */
        mem_size_aligned = end_align - begin_align - 2 * SIZEOF_STRUCT_MEM;

    }else{

        PT_DEBUG_LOG ( PT_LOG_LEVEL_DEBUG, ( "pt_system_heap_init ERR\n" ) );
        return;
    }

    /* point to begin address of heap */
    heap_ptr = (uint8_t *)begin_align;
    /* initialize the start of the heap */
    mem        = (struct heap_mem *)heap_ptr;
    mem->magic = HEAP_MAGIC;
    mem->next  = mem_size_aligned + SIZEOF_STRUCT_MEM;	//初始化指向堆底部
    mem->prev  = 0;
    mem->used  = 0;

    /* initialize the end of the heap */
    heap_end        = (struct heap_mem *)&heap_ptr[mem->next];
    heap_end->magic = HEAP_MAGIC;
    heap_end->used  = 1;
    heap_end->next  = mem_size_aligned + SIZEOF_STRUCT_MEM;
    heap_end->prev  = mem_size_aligned + SIZEOF_STRUCT_MEM;


//    rt_sem_init(&heap_sem, "heap", 1, PT_IPC_FLAG_FIFO);

    /* initialize the lowest-free pointer to the start of the heap */
    lfree = (struct heap_mem *)heap_ptr;
}

/**
 * Allocate a block of memory with a minimum of 'size' bytes.
 *
 * @param size is the minimum size of the requested block in bytes.
 *
 * @return pointer to allocated memory or NULL if no free memory was found.
 */
void * _pt_malloc ( uint32_t size )
{
    PT_ASSERT(PT_NULL != heap_ptr);
    PT_ASSERT(PT_NULL != lfree);
    PT_ASSERT(PT_NULL != heap_end);

	uint32_t ptr, ptr2;
	struct heap_mem * mem, *mem2;

	if ( size == 0 ) {
		return PT_NULL;
	}

//	if ( size != PT_ALIGN ( size, PT_ALIGN_SIZE ) )
//		RT_DEBUG_LOG ( RT_DEBUG_MEM, ( "malloc size %d, but align to %d\n",
//									   size, PT_ALIGN ( size, PT_ALIGN_SIZE ) ) );
//
//	else {
//		RT_DEBUG_LOG ( RT_DEBUG_MEM, ( "malloc size %d\n", size ) );
//	}

	/* alignment size */
	size = PT_ALIGN ( size, PT_ALIGN_SIZE );

	if ( size > mem_size_aligned ) {
		PT_DEBUG_LOG ( PT_LOG_LEVEL_DEBUG, ( "no memory\n" ) );

		return PT_NULL;
	}

	/* every data block must be at least MIN_SIZE_ALIGNED long */
	if ( size < MIN_SIZE_ALIGNED ) {
		size = MIN_SIZE_ALIGNED;
	}

	/* take memory semaphore */
    //别的地方未释放内存堆资源, 直接返回
    if( pt_sem_take(&heap_sem) != PT_EOK ){
        return PT_NULL;
    }

	for ( ptr = ( uint8_t * ) lfree - heap_ptr;
		  ptr < mem_size_aligned - size;
		  ptr = ( ( struct heap_mem * ) &heap_ptr[ptr] )->next ) {

		mem = ( struct heap_mem * ) &heap_ptr[ptr];

		//这块内存没有被用, 而且距离下一块内存的大小大于需求
		if ( ( !mem->used ) && ( mem->next - ( ptr + SIZEOF_STRUCT_MEM ) ) >= size ) {
			/* mem is not used and at least perfect fit is possible:
			 * mem->next - (ptr + SIZEOF_STRUCT_MEM) gives us the 'user data size' of mem */
			//到一下个内存块的空间比需求的值加上固有的头占用 还大
			if ( mem->next - ( ptr + SIZEOF_STRUCT_MEM ) >=
				 ( size + SIZEOF_STRUCT_MEM + MIN_SIZE_ALIGNED ) ) {
				/* (in addition to the above, we test if another struct heap_mem (SIZEOF_STRUCT_MEM) containing
				 * at least MIN_SIZE_ALIGNED of data also fits in the 'user data space' of 'mem')
				 * -> split large block, create empty remainder,
				 * remainder must be large enough to contain MIN_SIZE_ALIGNED data: if
				 * mem->next - (ptr + (2*SIZEOF_STRUCT_MEM)) == size,
				 * struct heap_mem would fit in but no data between mem2 and mem2->next
				 * @todo we could leave out MIN_SIZE_ALIGNED. We would create an empty
				 *       region that couldn't hold data, but when mem->next gets freed,
				 *       the 2 regions would be combined, resulting in more free memory
				 */
				ptr2 = ptr + SIZEOF_STRUCT_MEM + size;

				/* create mem2 struct */
				mem2       = ( struct heap_mem * ) &heap_ptr[ptr2];
				mem2->magic = HEAP_MAGIC;
				mem2->used = 0;
				mem2->next = mem->next;		//指向上一块的next
				mem2->prev = ptr;			//上一块

				/* and insert it between mem and mem->next */
				mem->next = ptr2;			//上一块的next指向新建的内存块
				mem->used = 1;				//上一块被使用了

				//下一块没到底, 就把下一块的prev指向新建的这一块
				if ( mem2->next != mem_size_aligned + SIZEOF_STRUCT_MEM ) {
					( ( struct heap_mem * ) &heap_ptr[mem2->next] )->prev = ptr2;
				}

			} else {
				/* (a mem2 struct does no fit into the user data space of mem and mem->next will always
				 * be used at this point: if not we have 2 unused structs in a row, plug_holes should have
				 * take care of this).
				 * -> near fit or excact fit: do not split, no mem2 creation
				 * also can't move mem->next directly behind mem, since mem->next
				 * will always be used at this point!
				 */
				mem->used = 1;
			}

			/* set memory block magic */
			mem->magic = HEAP_MAGIC;

			//当前这块是空闲内存块
			if ( mem == lfree ) {
				/* Find next free block after mem and update lowest free pointer */
				while ( lfree->used && lfree != heap_end ) {
					lfree = ( struct heap_mem * ) &heap_ptr[lfree->next];
				}

				PT_ASSERT ( ( ( lfree == heap_end ) || ( !lfree->used ) ) );
			}

            pt_sem_signal(&heap_sem);  //释放内存堆的占用

			PT_ASSERT ( ( uint32_t ) mem + SIZEOF_STRUCT_MEM + size <= ( uint32_t ) heap_end );
			PT_ASSERT ( ( uint32_t ) ( ( uint8_t * ) mem + SIZEOF_STRUCT_MEM ) % PT_ALIGN_SIZE == 0 );
			PT_ASSERT ( ( ( ( uint32_t ) mem ) & ( PT_ALIGN_SIZE - 1 ) ) == 0 );


			/* return the memory data except mem struct */
			return ( uint8_t * ) mem + SIZEOF_STRUCT_MEM;
		}
	}

    pt_sem_signal(&heap_sem);  //退出时释放内存堆的占用
	return PT_NULL;
}


//========================== 防止分配后忘记释放的警告 =================
#define MEM_STAT_MAX    32

struct mem_stat_str     mem_stat[MEM_STAT_MAX];
uint32_t get_mem_stat_max(void)
{
    return MEM_STAT_MAX;
}

struct mem_stat_str * get_mem_stat(void)
{
    return mem_stat;
};

extern struct pt *get_cur_thread(void);
// 若无需警告, 分配结束后调用这个函数
void pt_mem_no_warning(void)
{
    struct pt   *cur_thread = get_cur_thread();
    int cur_null_idx = -1;
    // 找到当前要释放分配的对应线程
    for (int i = 0; i < MEM_STAT_MAX; i++){
        if(cur_thread == mem_stat[i].thread){
            mem_stat[i].thread = PT_NULL;
            cur_null_idx = i;
            break;
        }
    }
    // 没找到
    if (cur_null_idx < 0){
        return;
    }
    // 将空的格子与后端非空的那个盒子交换
    for (int i = MEM_STAT_MAX - 1; i >= 0; i--){
        if(mem_stat[i].thread != PT_NULL){
            mem_stat[cur_null_idx].thread = mem_stat[i].thread;
            mem_stat[i].thread = PT_NULL;
            break;
        }
    }
}


void * pt_malloc ( uint32_t size )
{
    void *p = _pt_malloc(size);
    if (p != PT_NULL){
        for (int i = 0; i < MEM_STAT_MAX; i++){
            if (mem_stat[i].thread == PT_NULL){
                mem_stat[i].thread = get_cur_thread();
                break;
            }

        }

    }else{
        PT_LOG_CALLER_ADDR0();  // 未分配成功, 打印返回地址
    }
    return p;
}
void * kernel_malloc ( uint32_t size )
{
    void *p = _pt_malloc(size);
    if (p == PT_NULL){
        PT_DEBUG_LOG ( PT_LOG_LEVEL_KERNEL, ( "kernel_malloc failed\n" ) );
        PT_LOG_CALLER_ADDR0();  // 未分配成功, 打印返回地址
    }
    return p;
}

/**
 * This function will contiguously allocate enough space for count objects
 * that are size bytes of memory each and returns a pointer to the allocated
 * memory.
 *
 * The allocated memory is filled with bytes of value zero.
 *
 * @param count number of objects to allocate
 * @param size size of the objects to allocate
 *
 * @return pointer to allocated memory / NULL pointer if there is an error
 */
void *pt_calloc(uint32_t count, uint32_t size)
{
    void *p;

    /* allocate 'count' objects of size 'size' */
    p = _pt_malloc(count * size);

    /* zero the memory */
    if (p){
//        pt_memset(p, 0, count * size);
        memclear(p, count * size);
    }

    return p;
}



static void plug_holes ( struct heap_mem * mem )
{
	struct heap_mem * nmem;
	struct heap_mem * pmem;

	PT_ASSERT ( ( uint8_t * ) mem >= heap_ptr );
	PT_ASSERT ( ( uint8_t * ) mem < ( uint8_t * ) heap_end );
	PT_ASSERT ( mem->used == 0 );

	/* plug hole forward */
	nmem = ( struct heap_mem * ) &heap_ptr[mem->next];

	if ( mem != nmem &&
		 nmem->used == 0 &&
		 ( uint8_t * ) nmem != ( uint8_t * ) heap_end ) {
		/* if mem->next is unused and not end of heap_ptr,
		 * combine mem and mem->next
		 */
		if ( lfree == nmem ) {
			lfree = mem;
		}

		mem->next = nmem->next;
		( ( struct heap_mem * ) &heap_ptr[nmem->next] )->prev = ( uint8_t * ) mem - heap_ptr;
	}

	/* plug hole backward */
	pmem = ( struct heap_mem * ) &heap_ptr[mem->prev];

	if ( pmem != mem && pmem->used == 0 ) {
		/* if mem->prev is unused, combine mem and mem->prev */
		if ( lfree == mem ) {
			lfree = pmem;
		}

		pmem->next = mem->next;
		( ( struct heap_mem * ) &heap_ptr[mem->next] )->prev = ( uint8_t * ) pmem - heap_ptr;
	}
}

/**
 * This function will change the previously allocated memory block.
 *
 * @param rmem pointer to memory allocated by rt_malloc
 * @param newsize the required new size
 *
 * @return the changed memory block address
 */
void *pt_realloc(void *rmem, uint32_t newsize)
{
    uint32_t size;
    uint32_t ptr, ptr2;
    struct heap_mem *mem, *mem2;
    void *nmem;

    /* alignment size */
    newsize = PT_ALIGN(newsize, PT_ALIGN_SIZE);
    if (newsize > mem_size_aligned){
        return PT_NULL;

    }else if (newsize == 0){
        pt_free(rmem);
        return PT_NULL;
    }

    /* allocate a new memory block */
    if (rmem == PT_NULL){
        return _pt_malloc(newsize);
    }

    //别的地方未释放内存堆资源, 直接返回
    if( pt_sem_take(&heap_sem) != PT_EOK ){
        return PT_NULL;
    }

    if ((uint8_t *)rmem < (uint8_t *)heap_ptr ||
        (uint8_t *)rmem >= (uint8_t *)heap_end){
        /* illegal memory */
        pt_sem_signal(&heap_sem);

        return rmem;
    }

    mem = (struct heap_mem *)((uint8_t *)rmem - SIZEOF_STRUCT_MEM);

    ptr = (uint8_t *)mem - heap_ptr;
    size = mem->next - ptr - SIZEOF_STRUCT_MEM;     // 原来的大小

    // 新的和原来的一样
    if (size == newsize){
        /* the size is the same as */
        pt_sem_signal(&heap_sem);

        return rmem;
    }

    // 本次调整缩小
    if (newsize + SIZEOF_STRUCT_MEM + MIN_SIZE < size){
        /* split memory block */

        ptr2 = ptr + SIZEOF_STRUCT_MEM + newsize;
        mem2 = (struct heap_mem *)&heap_ptr[ptr2];
        mem2->magic = HEAP_MAGIC;
        mem2->used = 0;
        mem2->next = mem->next;
        mem2->prev = ptr;

        mem->next = ptr2;
        if (mem2->next != mem_size_aligned + SIZEOF_STRUCT_MEM){
            ((struct heap_mem *)&heap_ptr[mem2->next])->prev = ptr2;
        }

        plug_holes(mem2);

        pt_sem_signal(&heap_sem);

        return rmem;
    }
    pt_sem_signal(&heap_sem);

    // 本次调整扩大
    /* expand memory */
    nmem = _pt_malloc(newsize);
    if (nmem != PT_NULL){ /* check memory */
        memcpy(nmem, rmem, size < newsize ? size : newsize);    // 将原来的内容拷贝到新分配的内存块中
        pt_free(rmem);  // 释放原来的内存块
    }

    return nmem;
}

/**
 * This function will release the previously allocated memory block by
 * pt_malloc. The released memory block is taken back to system heap.
 *
 * @param rmem the address of memory which will be released
 * @return error code
 */
int32_t pt_free ( void * rmem )
{
	struct heap_mem * mem;

    //无效参数
	if ( rmem == PT_NULL ) {
		return PT_EINVAL;
	}

	// 地址未对齐 PT_ALIGN_SIZE
	PT_ASSERT ( ( ( ( uint32_t ) rmem ) & ( PT_ALIGN_SIZE - 1 ) ) == 0 );
	//处于初始化时的堆区域内
//	PT_ASSERT ( ( uint8_t * ) rmem >= ( uint8_t * ) heap_ptr &&
//				( uint8_t * ) rmem < ( uint8_t * ) heap_end );

    //不在堆区域内, 返回无效参数
	if ( ( uint8_t * ) rmem < ( uint8_t * ) heap_ptr ||
		 ( uint8_t * ) rmem >= ( uint8_t * ) heap_end ) {

		PT_DEBUG_LOG ( PT_LOG_LEVEL_KERNEL, ( "illegal memory\n" ) );

		return PT_EINVAL;
	}

	/* Get the corresponding struct heap_mem ... */
	//找到该内存块的头部
	mem = ( struct heap_mem * ) ( ( uint8_t * ) rmem - SIZEOF_STRUCT_MEM );

//	RT_DEBUG_LOG ( RT_DEBUG_MEM,
//				   ( "release memory 0x%x, size: %d\n",
//					 ( uint32_t ) rmem,
//					 ( uint32_t ) ( mem->next - ( ( uint8_t * ) mem - heap_ptr ) ) ) );


	/* protect the heap from concurrent access */
    //别的地方未释放内存堆资源, 返回无资源
    if( pt_sem_take(&heap_sem) != PT_EOK){
        return PT_EEMPTY;
    }

	/* ... which has to be in a used state ... */
	//没被使用或者幻数被破坏了
	if ( !mem->used || mem->magic != HEAP_MAGIC ) {
        PT_ASSERT(0);
	}

	PT_ASSERT ( mem->used );
	PT_ASSERT ( mem->magic == HEAP_MAGIC );
	/* ... and is now unused. */
	mem->used  = 0;
	mem->magic = HEAP_MAGIC;


	if ( mem < lfree ) {
		/* the newly freed struct is now the lowest */
		lfree = mem;
	}


	/* finally, see if prev or next are free also */
	plug_holes ( mem );
    pt_sem_signal(&heap_sem);  //退出时释放内存堆的占用

    pt_mem_no_warning();

    return PT_EOK;
}

bool is_heap_mem( void * rmem )
{
    //不在堆区域内, 返回无效参数
	if ( ( uint8_t * ) rmem < ( uint8_t * ) heap_ptr ||
		 ( uint8_t * ) rmem >= ( uint8_t * ) heap_end ) {

		return false;
	}else{
        return true;
	}

}

#endif /* PT_USING_MEM_HEAP */
